﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Input;
using System.Windows.Media;
using Bling.DSL;
using Bling.Properties;
using Bling.Util;
using Bling.Physics;
using Bling.WPF;
using Bling.Geom;
using Bling.Animation;

namespace Bling.Physics {
  public class SpatialHash<T> {
    private interface HasNextNode {
      Node Next { get; set; }
      string ToShortString();
    }
    private class Node : HasNextNode {
      public T Item;
      public Node Next { get; set; }
      public override string ToString() {
        return Item + " next=" + Next.ToShortString();
      }
      public string ToShortString() { return "N" + Item + ":" + (GetHashCode() % 100); }
    }
    private class HeadNode : HasNextNode {
      public Node Next { get; set; }
      public string ToShortString() { return "HD"; }
    }
    // list of nodes
    private readonly HeadNode Head = new HeadNode();
    public void Insert(T Item) {
      var node = new Node() { Item = Item, Next = Head.Next };
      Head.Next = node;
    }
    public Func<T, bool> IsAlive { private get; set; }
    public Func<T, Point> LeftTop { private get; set; }
    public Func<T, Point> RightBottom { private get; set; }

    public void DoHash(Point sz) {
      for (int i = 0; i < Table.GetLength(0); i++)
        for (int j = 0; j < Table.GetLength(1); j++)
          Table[i, j].Clear();
      var stepX = sz.X / Table.GetLength(0);
      var stepY = sz.Y / Table.GetLength(1);

      for (var at = Head.Next; at != null; at = at.Next) {
        var start = LeftTop(at.Item);
        var stop = RightBottom(at.Item);

      }

    }

    public SpatialHash(World world, int NX, int NY) {
      this.IsAlive = (t) => true;
      this.Table = new List<T>[NX, NY];
      for (int i = 0; i < NX; i++) 
        for (int j = 0; j < NY; j++)
          Table[i, j] = new List<T>();
    }
    private readonly List<T>[,] Table;
  }
  public class SweepList<T> {
    private interface HasNextNode {
      HasPrevNode Next { get; set; }
      string ToShortString();
    }
    private interface HasPrevNode {
      HasNextNode Prev { get; set; }
      string ToShortString();
    }
    private class Node : HasNextNode, HasPrevNode {
      public T Item;
      public HasPrevNode Next { get; set; }
      public HasNextNode Prev { get; set; }

      public Node Temp;

      public override string ToString() {
        return Item + " prev=" + Prev.ToShortString() + " next=" + Next.ToShortString();
      }
      public string ToShortString() { return "N" + Item + ":" + (GetHashCode() % 100); }
    }
    private class HeadNode : HasNextNode {
      public HasPrevNode Next { get; set; }
      public string ToShortString() { return "HD"; }
    }
    private class TailNode : HasPrevNode {
      public HasNextNode Prev { get; set; }
      public string ToShortString() { return "TL"; }
    }
    private readonly HeadNode Head = new HeadNode();
    private readonly TailNode Tail = new TailNode();
    public SweepList(World World) {
      Head.Next = Tail;
      Tail.Prev = Head;
      Active = (t) => true;
      BeginCollide = () => { };
      EndCollide = () => { };
      if (World != null) World.AfterRelax.Add(() => {
        if (true) {
          int i = 0;
          while (true) {
            if (i == 3) break; // give up on being completed sorted.
            if (!this.BubbleSortForward()) break;
            if (!this.BubbleSortBackward()) break;
            i++;
          }
          this.BeginCollide();
          this.Sweep();
          this.EndCollide();
        }
      });
    }
    public bool Sort() {
      return BubbleSortForward() && BubbleSortBackward();

    }

    public void Init(Func<T, double> Start, Func<T, double> End) {
      this.Compare = (t0, t1) => Start(t0) - Start(t1);
      this.CompareEnd = (t0, t1) => End(t0) - End(t1);
      this.CanCollide = (t0, t1) => End(t0) > Start(t1);
    }


    public Func<T, T, double> Compare { private get; set; }
    public Func<T, T, double> CompareEnd { private get; set; }
    public Func<T, bool> Active { private get; set; }
    public Func<T, T, bool> CanCollide { get; set; }
    public Action BeginCollide { private get; set; }
    public Action<T, T> DoCollide { private get; set; }
    public Action EndCollide { private get; set; }
    public void Insert(T t) {
      HasNextNode at = Head;
      while (at.Next is Node) {
        var node = (Node)at.Next;
        if (Compare(t, node.Item) < 0) {
          InsertAfter(at, new Node() { Item = t });
          return;
        }
        at = (Node) at.Next;
      }
      // at the end.
      InsertBefore(Tail, new Node() { Item = t });
    }


    public bool BubbleSortForward() {
      bool changed = false;
      HasNextNode at = Head;
      while (at.Next is Node) {
        var next = (Node)at.Next;
        (next.Prev == at).Assert();
        if (!Active(next.Item)) {
          Delete(next);
          (at.Next != next).Assert();
        } else {
          if (at is Node) {
            var prev = (Node)at;
            if (Compare(next.Item, prev.Item) < 0) {
              // swap.
              (prev.Next == next).Assert();
              (next.Prev == prev).Assert();
              prev.Prev.Next = next;
              next.Next.Prev = prev;

              prev.Next = next.Next;
              next.Prev = prev.Prev;
              prev.Prev = next;
              next.Next = prev;
              next = prev;
              changed = true;
            }
          }
          at = next;
        }
      }
      return changed;
    }
    public bool BubbleSortBackward() {
      bool changed = false;
      HasPrevNode at = Tail;
      while (at.Prev is Node) {
        var prev = (Node)at.Prev;
        (prev.Next == at).Assert();
        if (!Active(prev.Item)) {
          Delete(prev);
          (at.Prev != prev).Assert();
        } else {
          if (at is Node) {
            var next = (Node)at;
            if (Compare(next.Item, prev.Item) < 0) {
              // swap.
              (prev.Next == next).Assert();
              (next.Prev == prev).Assert();
              prev.Prev.Next = next;
              next.Next.Prev = prev;

              prev.Next = next.Next;
              next.Prev = prev.Prev;
              prev.Prev = next;
              next.Next = prev;
              next = prev;
              changed = true;
            }
          }
          at = prev;
        }
      }
      return changed;
    }
    public void Sweep<S>(SweepList<S> other, Func<S, T, int> CanCollide, Action<T, S> DoCollide) {
      HasNextNode at = Head;
      SweepList<S>.Node InPlay = null;
      SweepList<S>.HasNextNode atS = other.Head;
      while (at.Next is Node) {
        var node = (Node)at.Next;
        while (InPlay != null && CanCollide(InPlay.Item, node.Item) > 0) 
          InPlay = InPlay.Temp;
        while (atS.Next is SweepList<S>.Node) {
          var nodeS = (SweepList<S>.Node)atS.Next;
          var n = CanCollide(nodeS.Item, node.Item);
          if (n < 0) break;
          if (n == 0) { // add to list
            if (InPlay == null || other.CompareEnd(nodeS.Item, InPlay.Item) <= 0) {
              nodeS.Temp = InPlay;
              InPlay = nodeS;
            } else {
              var at0 = InPlay;
              while (true) {
                if (at0.Temp == null || other.CompareEnd(nodeS.Item, at0.Temp.Item) <= 0) {
                  nodeS.Temp = at0.Temp;
                  at0.Temp = nodeS;
                  break;
                } else at0 = at0.Temp;
              }
            }

          }
          atS = nodeS;
        }
        // all of in play nodes we can collide with. 
        for (var x = InPlay; x != null; x = x.Temp) DoCollide(node.Item, x.Item);
        at = node;
      }



    }
    public void Sweep(Func<T, T, bool> CanCollide, Action<T, T> DoCollide) {
      HasNextNode at = Head;
      Node InPlay = null; // no node in play at first. 
      while (at.Next is Node) {
        var node = (Node)at.Next;
        // adjust the list. 
        while (InPlay != null && !CanCollide(InPlay.Item, node.Item))
          InPlay = InPlay.Temp;
        // check for collisions
        for (Node n = InPlay; n != null; n = n.Temp)
          DoCollide(n.Item, node.Item);
        // sort node into InPlay list.
        node.Temp = null;
        if (InPlay == null || CompareEnd(node.Item, InPlay.Item) <= 0) {
          node.Temp = InPlay;
          InPlay = node;
        } else {
          var at0 = InPlay;
          while (true) {
            if (at0.Temp == null || CompareEnd(node.Item, at0.Temp.Item) <= 0) {
              node.Temp = at0.Temp;
              at0.Temp = node;
              break;
            } else at0 = at0.Temp;
          }
        }
        at = node;
      }
    }
    public void Sweep() {
      Sweep(CanCollide, DoCollide);
    }

    private void Delete(Node node) {
      node.Prev.Next = node.Next;
      node.Next.Prev = node.Prev;
    }

    private void InsertAfter(HasNextNode At, Node Node) {
      var OldNext = At.Next;
      (OldNext.Prev == At).Assert();
      At.Next = Node;
      OldNext.Prev = Node;
      Node.Prev = At;
      Node.Next = OldNext;
    }
    private void InsertBefore(HasPrevNode At, Node Node) {
      InsertAfter(At.Prev, Node);
    }
  }

  public static class CollisionBody {
    /// <summary>
    /// Access extension for this block.
    /// </summary>
    public static Impl<BLOCK> CollisionBodyE<BLOCK>(this PhysicsBlock<BLOCK> block)
      where BLOCK : PhysicsBlock<BLOCK>, Is<BLOCK>, ParticleBody.Is<BLOCK> {
      return block.Extension<Impl<BLOCK>>();
    }
    public interface BaseBlock {}

    /// <summary>
    /// A block extends this if they implement the containing extension.
    /// </summary>
    public interface Is<BLOCK> : HasBlockExtension<BLOCK, Impl<BLOCK>>, BaseBlock where BLOCK : PhysicsBlock<BLOCK>, Is<BLOCK>, ParticleBody.Is<BLOCK> { }

    public interface BaseImpl {
      BaseCollisionBody[] Bodies { get; }
    }

    /// <summary>
    /// State for containing extension.
    /// </summary>
    public class Impl<BLOCK> : BlockExtension<Impl<BLOCK>, BLOCK>, BaseImpl where BLOCK : PhysicsBlock<BLOCK>, Is<BLOCK>, ParticleBody.Is<BLOCK> {
      private readonly CustomBody[] Bodies0;
      internal readonly BLOCK Block;
      public Impl(BLOCK block) : base(block) {
        this.Block = block;
        Bodies0 = new CustomBody[block.Count];
        // initialize the array. 
        for (int i = 0; i < block.Count; i++)
          Bodies0[i] = new CustomBody() { Impl = this, Index = i };
      }
      public BaseCollisionBody[] Bodies { get { return Bodies0; } }

      protected internal override void DoInit(BLOCK block) {
        ForceInit<ParticleBody.Impl<BLOCK>>(block);
      }
      private Func<int, Rect> AABBF;
      private Func<int, int, Point> PositionF;
      private Action<int, int, Point> SetPositionF;
      private int ParticleCount;
      protected internal override void DoActivate(BLOCK block) {
        base.DoActivate(block);
        ParticleCount = block.ParticleCount();
        PositionF = PointBl.Func<int, int>((i, j) => block[i].Particles()[j].Position);
        SetPositionF = PointBl.Assign<int, int, Point>((i, j, p) => block[i].Particles()[j].Position, (i, j, p) => p);
        MassF = DoubleBl.Func<int>(i => Mass(block[i]));

        var AABB0 = Point4DBl.Func<int>((i) => block[i].AABB());
        AABBF = i => {
          var p = AABB0(i);
          return new Rect(new Point(p.X, p.Y), new Size(p.Z, p.W));
        };

      }
      public Func<PhysicsBlock<BLOCK>.Body, DoubleBl> Mass = (idx) => 1d;
      private Func<int, double> MassF;

      public class CustomBody : BaseCollisionBody {
        internal Impl<BLOCK> Impl;
        internal int Index;
        public override PhysicsBlock Block {
          get { return Impl.Block; }
        }
        public override double Mass {
          get { return Impl.MassF(Index); }
        }
        public override int CoreIndex {
          get { return Index; }
        }
        private Rect AABB0 = new Rect(0, 0, 0, 0);
        public override void Prepare() {
          base.Prepare();
          AABB0 = Impl.AABBF(Index);
        }
        public override Rect AABB { get { return AABB0; } }
        public override Point Center { get { return AABB0.TopLeft + ((Vector) AABB0.Size) / 2d; } }
        public override int Count {
          get { return Impl.ParticleCount; }
        }
        public override Point this[int ParticleIndex] {
          get {
            while (ParticleIndex < 0) ParticleIndex = Impl.ParticleCount + ParticleIndex;
            return Impl.PositionF(Index, ParticleIndex % Impl.ParticleCount); 
          }
          set {
            while (ParticleIndex < 0) ParticleIndex = Impl.ParticleCount + ParticleIndex;
            Impl.SetPositionF(Index, ParticleIndex % Impl.ParticleCount, value); 
          }
        }
      }
    }
  }
  public class BodyCollisionManager {
    internal Point pAStart;
    internal Point pAEnd;
    internal Point pBStart;
    internal Point pBEnd;
    internal Point pC;
    private readonly List<CollisionBody.BaseImpl> Installed = new List<CollisionBody.BaseImpl>();
    public readonly SweepList<BaseCollisionBody> XSort = new SweepList<BaseCollisionBody>(null);


    public BodyCollisionManager(CanvasBl Canvas, World World) {
      World.AfterRelax.Add(() => {
        DoCollisions(true);
      });
      if (false) {
        var LineA = new LineBl(Canvas) { Stroke = { Brush = Brushes.Red, Thickness = 1 }, ZIndex = 100, IsHitTestVisible = false };
        var LineB = new LineBl(Canvas) { Stroke = { Brush = Brushes.Blue, Thickness = 1 }, ZIndex = 100, IsHitTestVisible = false };
        var Thumb = new ThumbBl(Canvas) { Background = Brushes.Green, ZIndex = 101, IsHitTestVisible = false };
        World.UpdateInUI.Add(() => {
          LineA.Start.Now = pAStart;
          LineA.End.Now = pAEnd;
          LineB.Start.Now = pBStart;
          LineB.End.Now = pBEnd;
          Thumb.CenterPosition = pC;
        });
      }
      XSort.Init(body => body.AABB.X, body => body.AABB.X + body.AABB.Width);
      XSort.DoCollide = (bodyA,bodyB) => {
        if (!bodyA.AABB.IntersectsWith(bodyB.AABB) || bodyA.Mass == 0 || bodyB.Mass == 0) return;
        bodyA.Collide(this, bodyB, true);
      };
    }

    public void Add<BLOCK>(PhysicsBlock<BLOCK> block) where BLOCK : PhysicsBlock<BLOCK>, CollisionBody.Is<BLOCK>, ParticleBody.Is<BLOCK> {
      Installed.Add(block.CollisionBodyE());
      for (int i = 0; i < block.CollisionBodyE().Bodies.Length; i++)
        XSort.Insert(block.CollisionBodyE().Bodies[i]);
    }
    public void Prepare() {
      for (int iA = 0; iA < Installed.Count; iA++) {
        var bA = Installed[iA].Bodies;
        for (int jA = 0; jA < bA.Length; jA++) {
          var bodyA = bA[jA];
          bodyA.Prepare();
        }
      }
      while (true) if (!XSort.Sort()) break;
    }

    public void DoCollisions(bool Apply) {
      Prepare();
      XSort.Sweep();
    }
  }
  public abstract class BaseCollisionBody {
    public abstract PhysicsBlock Block { get; }
    public abstract int CoreIndex { get; }
    public abstract Point this[int idx] { get; set; }
    public abstract int Count { get; }
    public abstract Point Center { get; }
    public virtual double Mass { get { return 1d; } }
    public abstract Rect AABB { get; }
    public virtual void Prepare() { }
    // brute force.
    public Vector Collide(BodyCollisionManager c, BaseCollisionBody other, bool apply) {
      // point-wise processing. 
      var adjust = new Vector(0, 0);
      var otherCenter = other.Center;
      var thisCenter = this.Center;
      for (int iA = 0; iA < this.Count; iA++) {
        for (int jA = 0; jA < other.Count; jA++) {
          Point? intersectA = null;
          {
            var piA0 = this[iA + 0];
            var piA1 = this[iA + 1];
            var pjA0 = other[iA + 0];
            var pjA1 = other[iA + 1];
            var result = PointBl.Intersect(this[iA + 0], this[iA + 1], other[jA + 0], other[jA + 1]);
            if (result != null) {
              c.pAStart= piA0;
              c.pAEnd = piA1;
              c.pBStart = pjA0;
              c.pBEnd= pjA1;
              c.pC = (Point)result;

              intersectA = result;
            }
          }
          if (intersectA == null) continue;
          bool iOrder, jOrder;
          {
            var lI0 = (this[iA + 0] - otherCenter).LengthSquared;
            var lI1 = (this[iA + 1] - otherCenter).LengthSquared;
            iOrder = lI0 > lI1;
            var lJ0 = (other[jA + 0] - thisCenter).LengthSquared;
            var lJ1 = (other[jA + 1] - thisCenter).LengthSquared;
            jOrder = lJ0 > lJ1;
          }
          var i0x = iOrder ? +1 : -1;
          var i1x = iOrder ? +2 : +0;
          var j0x = jOrder ? +1 : -1;
          var j1x = jOrder ? +2 : +0;
          var intersectB = PointBl.Intersect(this[iA + i0x], this[iA + i1x], other[jA + 0], other[jA + 1]);
          if (intersectB != null) {
            var A = this[iA + (iOrder ? 1 : 0)];
            Point B = (Point)intersectA, C = (Point)intersectB;
            var D = PointBl.TriangleBase(A, B, C);
            var diff = D.IsNan() ? (B - A) : (D - A);
            if (diff.X == 0 && diff.Y == 0) continue;
            //diff = diff;
            if (apply) {
              var total = this.Mass + other.Mass;
              this[iA + (iOrder ? 1 : 0)] += (diff * other.Mass / total);
              other[jA + 0] -= (diff * this.Mass / total);
              other[jA + 1] -= (diff * this.Mass / total);
            } else {
              adjust = adjust.AbsMax(diff);
            }
            continue;
          }
          intersectB = PointBl.Intersect(this[iA + 0], this[iA + 1], other[jA + j0x], other[jA + j1x]);
          if (intersectB != null) {
            var A = other[jA + (jOrder ? 1 : 0)];
            Point B = (Point)intersectA, C = (Point)intersectB;
            var D = PointBl.TriangleBase(A, B, C);
            var diff = D.IsNan() ? (B - A) : (D - A);
            if (diff.X == 0 && diff.Y == 0) continue;
            if (apply) {
              var total = this.Mass + other.Mass;
              other[jA + (jOrder ? 1 : 0)] += (diff * this.Mass / total);
              this[iA + 0] -= (diff * other.Mass / total);
              this[iA + 1] -= (diff * other.Mass / total);
            } else {
              adjust = adjust.AbsMax(-diff);
            }
            continue;
          }
          intersectB = PointBl.Intersect(this[iA + i0x], this[iA + i1x], other[jA + j0x], other[jA + j1x]);
          if (intersectB != null) {
            // a cross bar.
            Point B = (Point)intersectA, C = (Point)intersectB;
            var total = this.Mass + other.Mass;
            {
              var A = this[iA + (iOrder ? 1 : 0)];
              var D = PointBl.TriangleBase(A, B, C);
              var diff = D.IsNan() ? (B - A) : (D - A);
              if (apply) {
                this[iA + (iOrder ? 1 : 0)] += (diff * other.Mass / total);
              } else {
                adjust = adjust.AbsMax(diff / 2);
              }
            }
            {
              var A = other[jA + (jOrder ? 1 : 0)];
              var D = PointBl.TriangleBase(A, B, C);
              var diff = D.IsNan() ? (B - A) : (D - A);
              if (apply) {
                other[jA + (jOrder ? 1 : 0)] += (diff * this.Mass / total);
              } else {
                adjust = adjust.AbsMax(-diff / 2);
              }
            }
            continue;
          }
          {
            var total = this.Mass + other.Mass;
            var pass = false;
            var ix = iA + (iOrder ? 1 : 0);
            if (PointBl.Within(this[ix], other[jA - 1], other[jA + 0], otherCenter) ||
                PointBl.Within(this[ix], other[jA + 0], other[jA + 1], otherCenter) ||
                PointBl.Within(this[ix], other[jA + 1], other[jA + 2], otherCenter)) {
              if (apply) {
                //this[ix] = this[ix] + (intersectA - this[ix]);
                Vector toAdd = (Vector)(intersectA - ((Vector)this[ix]));
                this[ix] += (toAdd * other.Mass / total);
              } else {
                adjust = adjust.AbsMax((((Point)intersectA) - this[ix]) / 2);
              }
              pass = true;
            }
            var jx = jA + (jOrder ? 1 : 0);
            if (PointBl.Within(other[jx], this[iA - 1], this[iA + 0], thisCenter) ||
                PointBl.Within(other[jx], this[iA + 0], this[iA + 1], thisCenter) ||
                PointBl.Within(other[jx], this[iA + 1], this[iA + 2], thisCenter)) {
              if (apply) {
                Vector toAdd = (Vector)(intersectA - ((Vector)other[jx]));
                other[jx] += (toAdd * this.Mass / total);
              } else {
                adjust = adjust.AbsMax(-(((Point)intersectA) - this[ix]) / 2);
              }
              pass = true;
            }
            if (pass) continue;
          }
          if (true) continue;
        }
      }
      return adjust;
    }


  }

}